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Abstract. In this paper, we consider a geometric formalism for optimal control of under- 
actuated mechanical systems. Our techniques are an adaptation of the classical Skinner and 
Rusk approach for the case of Lagrangian dynamics with higher-order constraints. We study a 
regular case where it is possible to establish a symplectic framework and, as a consequence, to 
obtain a unique vector field determining the dynamics of the optimal control problem. These 
developments will allow us to develop a new class of geometric integrators based on discrete 
variational calculus. 



1. Introduction 

The mathematical activity in the last century in dynamical systems, mechanics and related 
areas has been extraordinary. The number of applications has grown exponentially and both 
basic science as well as several engineering technologies are profiting from this development. In 
the 1960s more sophisticated and powerful techniques coming from modern differential geometry 
and topology have been introduced in their study, experiencing a spectacular growth in the last 
50 years. Control and optimal control of mechanical systems has not ignored these developments, 
becoming now a principal research focus of nonlinear control theory. In particular, there are an 
increasing interest in the control of underactuated mechanical systems (see [101 US])- These type 
of mechanical systems are characterized by the fact that there are more degrees of freedom than 
actuators. This type of system is quite different from a mathematical and engineering perspective 
than fully actuated control systems where all the degrees of freedom are actuated. 

The class of underactuated mechanical systems are abundant in real life for different reasons, 
for instance, as a result of design choices motivated by the search of less cost engineering devices 
or as a result of a failure regime in fully actuated mechanical systems. The underactuated 
systems include spacecraft, underwater vehicles, mobile robots, helicopters, wheeled vehicles, 
mobile robots, underactuated manipulators... 

On the other hand, there are many papers in which optimal control problems are addressed 
using geometric techniques (see, for instance, [HI HSl [111 131] and references therein). Now, we 
introduce an optimization strategy in an underactuated mechanical system, that is, wc arc inter- 
ested in studying the implementation of devices in which a controlled quantity is used to influence 
the behavior of the undeactuated system in order to achieve a desired goal (control) using the 
most economical strategy (optimization). Thus, in our paper we develop a new geometric set- 
ting for optimal control of underatuated lagrangian systems strongly inspired on the Skinner 
and Rusk formulation for singular Lagrangians systems |30j . Since in this setting the controlled 
Euler-Lagrange equation are second-order differential equations we will need to implement an 
higher-order version of this classical Skinner and Rusk formalism [^ . This geometric procedure 
gives us an intrinsic version of the differential equations for optimal trajectories and permits us 
to detect the preservation of geometric properties (symplecticity, preservation of the hamiltonian. 
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etc.). For expository simplicity, we restrict ourselves in Section|5]to the so-called optimal control 
of superarticulated mechanical systems, in which only some of the degrees of freedom are con- 
trolled directly, with the remaining variables freely evolving subject only to dynamic interactions 
with the actuated degrees of freedom (see [21 ES]). Obviously, our theory can be easily extended 
to more general class of underactuated lagrangian systems. 

Moreover, using a discrete version of this variational approach to optimal control of under- 
actuated lagrangian systems it is possible to construct discretization schemes for this type of 
systems which preserve a discrete symplectic form. All this type of geometric integrators has 
demonstrated, in worked examples, an exceptionally good longtime behavior and obviously this 
research is of great interest from numerical and geometric considerations (see |17j ) . 

The paper is organized as follows. In Section [2] we recall some geometric constructions and 
properties of higher-order tangent bundles to make the paper self-contained. In section |3] we 
derive the main geometric objects of higher-order variational calculus with higher-order con- 
straints. In Section [4] we deduce the geometric framework for higher-order mechanics using the 
Skinner and Rusk formalism. In Section [s] we apply the previous techniques to the case of opti- 
mal control of underactuated Lagrangian systems. A concrete example: the Cart-Pole system is 
carefully analyzed. Finally, in Section [6] we derive the corresponding discrete version obtaining 
a numerical integrator which inherits some of the previously analyzed geometric properties of 
the continuous optimal control problem for the underactuated system (symplecticity, momentum 
preservation...). 



2. Higher-order tangent bundles 

In this section we recall some basic facts of the higher-order tangent bundles theory. For more 
details see [HI [21]. 

Let Q be a manifold of dimension n. An equivalence relation is introduced in the set C°°(M, Q) 
of differentiable curves from M to Q. By definition, two given curves in Q ji{t) and 72(t) where 
t S (—a, a) with a G M have contact of order k at qo = 7i(0) = 72(0) if there is a local chart 
{(f, U) of Q such that qq ^ U and 



(<^°7i(i)) = 377 (<^°72(i)) 



t=o dt^ 

for s — 0, k. This is a well defined equivalence relation in C°°(K, Q) and the equivalence class of 
a curve 7 will be denoted by [7]o'^^ . The set of equivalence classes will be denoted by T^'^^ Q and we 
can see that it is a differentiable manifold. Moreover, Tq : T'^^^'Q Q where Tq ^[7]o'^''^ — 7(0) 
is a fiber bundle called the tangent bundle of order k of Q. 



We also may define the surjective mappings Tq'^^ : T^^^Q — > T^^^Q, for / < fc, given by 



■^q'''^ (Wo''^) = Wo^- It is easy to see that T^^'^Q = TQ, the tangent bundle of Q, T^^'^Q = Q 
and T^Q-''^ = T^. 

Given a differentiable function f : Q — > M and / G {0, fc}, its /-lift to T'-'^^Q, < I < k, 
is the differentiable function defined as 

Of course, these definitions can be applied to functions defined on open sets of Q. 

Prom a local chart (g*) on a neighborhood U of Q, it is possible to induce local coordinates 
(^qm^qWi^__^q{k)^■^ on T^'')U ^ {t-q)~^{U), where q'-"'^' = (g»)(«.fc) if < s < fc. Sometimes, we 
will use the standard conventions, q^^^^'^ = g*, g^^^* = and g^^^* = q\ 

Given a vector field X on Q, we define its fc-lift X^^) to T'^'^^Q as the unique vector field on 
T^'^^Q satisfying the following identities 

X(k)(^f(i,k)-^ = (X(/))('''=) , 0<l<k, 
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for all difFerentiable function / on Q. In coordinates, the fc-lift of a vector field X = X^^—^ is 



Now, we consider the canonical immersion jk : T^^^Q — > T{T^^^^^Q) defined as jfc([7]o'''') = 
\ where 7('=~i' is the lift of the curve 7 to tC^^^'Q; that is, the curve 7(''~i) : M ^ 
T^'^^^'^Q is given by 7('^^^)(t) = [7t]o'^ where 7t(s) = 7(t + s). In local coordinates 

,(^)\ ...gC^)') = .^^^ . . . , q^'~'>- q^'^\ q^'^\ q^'^1 . 

We use the map jk to construct the differential operator dr which maps a function / on T^^^Q 
into a function ^t/ on T^^+^^Q 

rfT/([7]o+')=Jfc+i([7]S+')(/)- 

3. VARIATIONAL CALCULUS WITH HIGHER-ORDER CONSTRAINTS 

In this section, we briefly review the main notions of variational calculus with higher-order 
constraints. 

Let us consider a mechanical system whose dynamic is described by a Lagrangian 
R that depends on higher-order derivatives up to order k. Given two points x,y € T^'^^^'^Q we 
define the infinite-dimensional manifold G^'^{x,y) of 2fc-differentiable curves which connect x and 
y as 

e2'=(a;,y) = {c: [0,r] | c is c('=-i)(0) = a; and c^''-^) (T) ^ y} . 

Fixed a curve c in 6^*^(0;, y), the tangent space to C^'^{x,y) at c is given by 

T,e\x,y) = {X ■.[0,T]^TQ \X is C^''-\X{t)eT,(^t)Q, 
X(fc-i)(0) = and X'-'^-^^T) = 0} . 

Let us consider the action functional A on C^'^-curves in Q given by 

A: C^''{x,y) — > R 

c ^ j^L{c('^){t))dt. ^^-'^ 

Definition 3.1. Hamilton's principle. A curve c E G'^''{x,y) is a solution of the Lagrangian 
system determined by L : T'^^'^Q — > M if and only if c is a critical point of A. 

In order to find the critical points of ^1, we need to characterize the curves c such that 
dA{c){X) = for all X £ Tc€^^{x,y). Taking a family of curves e Q'^''{x,y) with co = c 
and e € (—6, b) C M, the stationary condition can be written as 

A{c,) = Q. 



de 

d 

Let us denote 5c 



e=0 

S 



cl and = —^Sc^; then we deduce the following result (see [H] 
de e=o dr 
for the same result when fc = 1). 

Theorem 3.2. Let L : T^^^Q R be a Lagrangian of order k and A{c) = ^"^ L{c^''\t))dt the 
action functional defined on C^'^(x,2/). Then, there exists a unique operator £i : T^'^^'^Q — >T*Q 
and a unique 1-form on T^'^^^^^Q such that for all variations 5c^ £ TcG^''{x,y) we have 



T 



dA{c)-5c,= / eL{c^^''\t)) ■ 6c{t) dt + eL{c^^'"^\t)) ■6^^''~^'>c{t) 







T 
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£.L is called Euler- Lagrange operator and Qj^ is called the Poincare-Cartan l-form. In 

local coordinates we have that 

fc-i 
/=o 

where the functions p^iyi with Q < I < k — 1, are the Jacobi- Ostrogradski generalized mo- 
menta defined by 

The equations of motion of the system, called higher-order Euler- Lagrange equations, 

are locally written as 



/=0 



The l-form Ql give rise the exact 2-form fl^ ~ —dQ^ on T^^'^ ^^Q which is called the 
Poincare Cartan 2-form. In local coordinates 

1=0 

Is easy to prove that fi^ is symplectic if and only if 

/ d^L \ 

The higher-order Lagrangian L is called regular if the 2-form fi^ is symplectic. In the following, 
we will assume that the Lagrangian L is regular. 

We will see that introducing higher-order constraints and applying the Lagrangian multipliers 
Lemma (see, for instance, flj), it is possible to derive the equations of motion and the corre- 
sponding geometric structures associated to this constrained problem. 

Let us consider a submanifold M of T^'^'^Q locally determined by the vanishing of the constraints 
functions : T^^'^Q ^ M, 1 < a < m. 

We assume that the restriction of the projection {tq '^'^'')\m ■ ^ ~^ T^''~^^Q is a submersion. 
Locally, this conditions means that the m x n-matrix 

is of rank m at all points of M. 

Consider now the subset C^'^(a;,y,M) of C'^''{x,y) of curves that satisfies these constraint 
equations, that is 

e^''{x,y,M) = {c:[0,T] — > Q \ q is C^'' , c^'"''^'> {0) = x , 

and c('=^i)(T) = y and c'^'^^t) £ M for alU e [0,T]}. 

Definition 3.3. A curve c e 6^*^(0;, y, M) will be called a solution of the higher-order variational 
problem with constraints if c is a critical point of A 

For solving this type of problems we will need the following version of the Lagrange Multipliers 
Lemma. 
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Lemma 3.4. Let be a smooth manifold and let 3^ be a Banach space with g : 'N ^ 3^ a smooth 
submersion so that g^^{0) is a submanifold ofN. Let / : Tsf ^ R &e a smooth function. Then 

X e g~^{0) is critical point of f 
point of f — \ o g. 



if and only if there exists A G 3^* such that x is a critical 



As in the case of systems with constraints on TQ, by using the Lagrange MultipHers lemma, 
we may characterize the regular critical points of the higher-order problem with constraints as 
an unconstrained problem for an extended Lagrangian system. (See |22j , for a detailed proof). 

Proposition 3.5. (Variational problem with higher-order constraints) 

A curve c € C^''{x,y,M.) is a critical point of the variational problem with higher-order con- 
straints if and only if c is a critical point of the functional 

^\{q^'^Ht),\{t))dt, 



where X = (Ai, . . . , Am) as regarded as generalized coordinates on M"" and L : T^^^Q 
is defined by 

i:(gW,gW,...,gW,A) = L(,("),gW,...,gW)-A„a>"(g("),gW,...,g« 
Remark 3.6. The equations 

k 

,m, 



^ / dL 9$" A 



<I>(g(°),gW,...,gW) = 0, 
are called Euler- Lagrange equations with higher-order constraints. 

4. Geometric formulation for higher-order constrained variational problems 

Now, we develop a geometric characterization higher-order constrained variational problems 
using, as an essential tool, the Skinner and Rusk formulation (see [5U]V 

Let us consider the Whitney sum T* {T'^^^^'^ Q) © T^'^'^Q and the canonical projections 

pri : T*{T^^-^'^Q)®T^^'^Q — > T*{T^^-^^Q), 

pr2 : T*{T^^-^'^Q) ® T^^^Q — > T'^'^^Q. 

Let us take the submanifold Wq — pr2^{M) = T*{T^'''^^Q) x M and the restrictions to Wq 
of the canonical projections pri and pr2 

TTi =pri,^^ : 1^0 cT*(r'=-iQ)®T(^-)Q^T*(T('=-i)Q) 

I Wo 

TT2 = pr2\,,, ■■ Wo C T*{T^-^Q) e T^^^Q ^ M . 

I Wo 

Now, we consider on Wq the presymplectic 2-form 

^Wo = '''i('^T('=-i)q): 

where u}x(k-i)Q is the canonical symplectic form on r*(T('=-i)Q). Define also the function Hwo ■ 
Wq — > M given by 

Hwo{a,p) = {a,jk{p)) - L\m{p) 
where {a,p) £ Wq ^ T*{T'-''~'^^Q) x M. Here (•,•) denotes the natural paring between vectors 
and covectors on T^^-'^^'Q (observe that jk{p) G TT^'^^^'Q). 

We will see that the dynamics of the higher-order constrained variational problem is intrinsi- 
cally characterized as the solutions of the presymplectic hamiltonian equation 

ix^Wo = dHwo ■ (4.1) 
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Let US consider n = pri* {LOj,(k-i)Q) and H : T* (T^'^-^'^Q) ® T'-'^^Q M given by 
H = {pri,pr2) -pr*2L = {pri,pr2) - Lott2- 

Observe that locally 

kerfi — span = ^ 



Then, it is easy to show that equations (4.1 ) are equivalent to (see [3]) 

1 X e TWo, 



(4.2) 



where TW'^ is the annihilator of TWq locally spanned by {d^"}, where : Wq M. denote the 
constraints <i>" = <i>" o pr2 (for notational simplicity, we do not distinguish the notation between 
constraints on M and constraints on M^o)- 

Take coordinates (9(0)% g(i)% . . . , q^*^"!)'; \ . . . ,pf qf'^)*) in T* {T^^-^^ Q) ® T'^^^i Q , then 
the local expressions of the presymplectic 2-forni and the haniiltonian H are 



fc-i 



r=0 



Consider a vector field X on T*{T^^ ^^Q) © T^'^^Q with local expression 

r=0 ^ r=0 <^-Pi 



The equations (4.2 1 implies that 



The solutions of Equation (4.2 1 are defined on the first constraint submanifold given by the 
set of points x £ Wq such that {dH + Xad^°'){x){Z) — 0, for all Z e kerri(x). Locally these 
restrictions are defined from the following relations 

1 (fe-i) dL d^^ 
^^^P^ -d^ + ^'^d^^^^ z-l,...,n. 

The equations (fj = (primary relations) determine the set of points Wi of Wq where ( [4.2^ has 
a solution. Wi is the primary constraint submanifold (assuming that it is a submanifold) for the 
presymplectic hamiltonian system (Wo^flwoiHwo)- (See, for instance, [TB]1. 

Then, we have two different types of equations which restrict the dynamics on T*{T^''~^^Q) (B 

rp(k)Q 



= a = 1 , . . . , m (constraints determining M) 
ip\ = i = 1, . . . , 71. (primary relations) 



(4.3) 
(4.4) 
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Therefore, the equations that define an integral curve of the solution X are given by 
^ = r = 0,...,fe-l, 



An 



*r _ KL ,„|£1_,<.-.. ,,5) 



and the constraint equations (14. 3| and (14.4 



Differentiating with respect to time the equations ip}, substituting in (4.5 1 and proceeding 
further, we find the equations of motion for the higher-order variational problem analyzed in 
Section [3] i.e. 



The solution of equation (4.11 on Wi may not be tangent to Wi. In such a case, we have 
to restrict Wi to the submanifold W2 where there exists at least a solution tangent to Wi. 
Proceeding further, we obtain a sequence of submanifolds [TB] (assuming that all the subsets 
generated by the algorithm are submanifolds) 

... ^Wk^ ■■■ ^W2^Wi--*Wo . 

Algebraically, these constraint submanifolds can be described as 

- e T* (r('=-i)Q) Xj.(.-i)Q M I dHw„{x){v) = Vi; G {T^W^-if } i>l, (4.7) 

where (T,W^,_i)^ = {v e T^Wq \ nwo{x){u,v) = Vu e T^W^-i }. 

If this constraint algorithm stabilizes, i.e., there exists a positive integer fc e N such that 
Wk+i — Wk and dimW^^ > 1, then we will have at least a well defined solution X on Wf — Wt 
such that 

{ix^Wo = dHwo)\W} ■ 

Now, denote by the puUback of presymplectic 2-form Q,Wo to Wi. In order to establish a 
necessary and sufficient condition for the symplecticity of the 2-form i we define as in Section 
[3j the extended lagrangian 

Si = L- A„$" . 

Theorem 4.1. For any choice of coordinates (q^'^^'' ,q^^^^ , . . . ^q'^''^^^'^; pf\ . . . ,p\'' ^\q''''^'') in 
T*(r('=-i)Q) ® tWQ, we have that {Wi,nwJ is a symplectic manifold if and only if 

gq(k)^g^{k)J gq{k)^ \ j dq^>''>^dqWj Qq(k)i gq{k)j Qq{k)i j _^ Q (^43^ 



(The proof follows the same lines that the one in Proposition 5.1 ). 

Remark 4.2. Observe that if the determinant of the matrix in Theorem |4.1| is not zero, then we 
can apply the implicit function theorem to the equation of constraints ip] — and = 0, and we 
can express the Lagrange multipliers Aq, and higher-order velocities q^*^-'* in terms of coordinates 
(g(«)%...,g('=-i)%pf\...,pf-^)),i.e., 

A. = A„(g(°),g«,...,9('=-i),p(°),...,p('=-i)), 
^ q(^^^iq("),qW,...,q('^-^),p("),...J'^-^)). 

Thus we can consider (q(°)\ g^^^*, . . . , g^^^^^^p-^', . . . ,pf'~^^) as local coordinates in Wi. In 
this case, 

r=0 
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which is obviously symplectic. 



5. Optimal Control of underactuated mechanical systems 

After introducing the geometry of higher-order Lagrangian system with constraints in the 
previous section, we may turn to the geometric framework for optimal control of underactuated 
mechanical systems. We recall that a Lagrangian control system is underactuated if the number 
of the control inputs is less than the dimension of the configuration space. We assume, in the 
sequel, that the considered systems are controllable ^lOj . 

Consider the class of underactuated Lagrangian control system (superarticulated mechanical 
system following the nomenclature by where the configuration space Q is the cartesian product 
of two differentiable manifolds, Q = Qi x Q2- Denote by {q'^) = {q°',q°'), 1 < A < n, local 
coordinates on Q where ((7°), 1 < a < r and {q"), 7' + 1 < a < n, are local coordinates on Qi 
and Q2, respectively. 

Given a Lagrangian L : TQ = TQi x TQ2 — > M, we assume that the controlled external forces 
can be applied only to the coordinates ((7°). Thus, the equations of motion are given by 



(5.1) 

= 0, 

az \ uq^ / oq"^ 
where a = 1, . . . , r, and a = r + 1, . . . , n. 

We study the optimal control problem that consists on finding a trajectory {q"^ (t) , q" (t) , u"'{t)) 







dL 


dt 


{dq-J 


d(f 


d_ 


(-) 


dL 


dt 


\dq'-) 


dq°' 



of state variables and control inputs satisfying equations (5.11 from given initial and final con- 
ditions, {q''{to),q°'{to),q°'{tQ),q"{to)), {q" {t j) , q" {t j) , q'^ {t f) , q"' {t f)) respectively, minimizing the 
cost functional 

A^ C{q%q'',q\r,u'')dt. 

Jto 

It is well know (see [T) that this optimal control problem is equivalent to the following con- 
strained variational problem. 

Extremize 



A= / L{q-{t),q^it),r{t),r{t),r{t),r{t))dt 

Jto 

subject to the second order constraints given by 

and the boundary conditions, where L : T^^^^Q M is defined as 

Now, according to the formulation given in Section [4] the dynamics of this second order 
constrained variational problem is determined by the solution of a presymplectic Hamiltonian 
system. In the following we repeat some of the constructions given in |4] but specialized to this 
particular setting, obtaining new insights for the optimal control problem under study. 

If M C r(2)Q is the submanifold given by annihilation of the functions we will see how to 
define local coordinates on M. 

From the constraint equations we have 
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Let us assume that the matrix (Wap) = (g^^^) is non-singular and denote by (W"^) its 
inverse. Thus, 

Therefore, we can consider {q^ , ^ q°^) as a system of local coordinates on M. The canonical 
inclusion ijvt : M ^ TTQ can be written as 

M TTQ 
{q\q\q'') ^ {q\ q\ q'^ , G'' {q\ q\ q'^)) . 

Define the restricted lagrangian L j^;: M ^ M. 



W^ = T*{TQ) xtqM 




T*TQ 



Figure 1. Second order Skinner and Rusk formalism 



We will consider Wq = T*{TQ) xtq M whose coordinates are {q^ , q^ \ , p\ , q"-) . 

Let us define the 2- form D.Wo = t^H^tq) on 14^0 and Hwo{ax,Vx) = {ax,iM{vx)) - Lyi{vx) 
where x £ TQ, G M^; = {{tq''^^)\'m)~'^{x) and ax G T*TQ. In local coordinates, 

^Wo = dq' A dp" + dq' A dp] , 

Hwo = pU'+Pir+piG"{q\q\r)-LM{q\q\qn- 

The dynamics of this variational constrained problem is determined by the solution of the 
equation 

ix^Wo = dHwo- (5.2) 
It is clear that is a presymplectic form on Wq and locally 

d 



ker fivKo = span 

Following the Gotay-Nester-Hinds algorithm we obtain the primary constraints 

d 



dHu 



= 



That is, 



dHwo 1 , 1 dG" dLj^ 

= Pa+Pa— — = 0- 



These new constraints (fl^ — give rise to a submanifold Wi of dimension An with local 
coordinates {q^ , q\ q°- , p^ , p^,) . 



Consider a solution curve {q'' {t) , q^ (t) , q°' {t) , p'^ (t) , p] (t)) of Equation (5.2|. Then, this curve 
satisfies the following system of differential equations 
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dq' 
~dt 
(Pq" 

dt 

dpi 
dt 



d'^q" 

~d^ 



= q 



. dq' d^q'^ 



-Pi 






dq^ 


dq^ 


-p° 


-Pi 


dq'- 


-pi 


dG" 




dq" 


dq'' 



dL 



dq' 



(5.3) 
(5.4) 
(5.5) 
(5.6) 
(5.7) 



From Equations (5.6) and (5.7) we deduce 



1 

Pa- 




dq°- 



idG' 

= -Pa - Pa 



dL 



M 



Qqa Qqa 



Differentiating with respect to time, replacing in the previous equahty and using (5.5) we obtain 
the following system of 4-order differential equations 




1 9G" 



dL 



Pa 



1 ac" 



Pa 







Also, using (|5.5|) and (|5.6|) we deduce 



dt^ 



dq"' 



^dGi^ 



dL 



1 dGf^ 



(5.8) 



(5.9) 



If we solve the implicit system of differential equations given by (5.8) and (5.9) then from Equa- 
tions (5.6) and (5.7) we deduce that the values of pjj and p„ are 



P'a = 



pI 



dLM 


1 


--( 


dLM 


dq'' 


^" dq'' 


dt \ 


dq" 


dLjA 


^dC^ 


_ '^Pi 




dq°' 


dq- ~ 


dt 





1 dG° 



(5.10) 



(5.11) 



Since, from our initial problem, we are only interested in the values q'^{t), it is uniquely 
necessary to solve the coupled system of implicit differential equations given by (5.8), (5.9) and 
(5.4) without expHcitly calculate the values Pa(t). 

Now, we are interested in the geometric properties of the dynamics. First, consider the sub- 
manifold Wi of Wo determined by 

{x e T*TQ Xtq M I dHwo{x){V) =OyVe kerr2(x)} 



Wi 

and the 2-form = iyvi^^a^ where iwi '■ Wi ^ denotes the canonical inclusion. Locally, 
Wi is determined by the vanishing of the constraint equations 



I 1,1 dG"^ dLj^ 



= 



Therefore, we can consider local coordinates {q'' , cf' , q"^ , Pi , pi) on Wi. 

Proposition 5.1. (Wi^flwi) is symplectic if and only if for any choice of local coordinates 
{q\q\ir,p°,p]) on Wq, 



det {"Jiab) = det 



d^L 



M 



dq^dtf 



1 a^G" 
^"'difdij^ 



^ along W\ 



(5.12) 



(n— r) X (n— r) 
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Proof: 

Let us recall that flwi is symplectic of and only if T^Wi n (T^Wi)'^ = Vx G Wi, where 
{T^Wif ^{ve T,{T*TQ) Xtq M) / flw„{x){v,w) = O,for all w G T,Wi} . 

Suppose that (Wi,flwi) is symplectic and that 

X°-Jlab{x) = for some e M. and x e Wi . 

Hence 

= . 



Therefore, A** ^ 



G TxWi but it is also in T^W^. This implies that Ab = for all b and that 
the matrix (JRab) is regular. 

Now, suppose that the matrix (Olab) is regular. Since 

8 



then. 



dij'' 



TxWi and, in consequence, 



TxWi e span 



= T,Wo. 



Now, let Z G TrM^i n (T^Wi) with x G M^i. It follows that 

d 



= iz^Woix) 



for all a and iz^Wo{x){Z) = 0, for all Z G T^rW^i 



Then, Z G \ieT:Q.Wo{x). This implies that 



Z=Xh 



dij'' 



Since Z G T^Wi then 



_9_ 



— Xb^ab 



and, consequently. At — 0, for all 6, and Z — 0. 



In the case where the matrix (5.121 is regular then the equations (5.8), (5.9 1 and (5.4) can be 



written as an explicit system of differential equations of the form 



d^q" 



. , dq^ d^ fq;^ ^ dpi 
dt' dt^ ' dt^ dt 



r<o.i ^ dq' d'^q'' 



d dL 



dt \ dq°' 



dL 



M 



dq°' 



(5.13) 
(5.14) 
(5.15) 



Remark 5.2. Now, we will analyze an alternative characterization of the condition (5.12) and 



its relationship with the matrix condition that appears in Theorem 4.1 Using the chain rule 



9Lm _ dL_ dL dG" 

dq°- ~ dij°^ ~^ 5^ dij" 

O'^Lm d^L d^L dGl^ d'^L dG" d"^! dG°' dG^ d"^! d'^G" 

— = 1 1 1 \ 

dq°'dq'' dq°-dq^ dq'^dql^ dq'' dq°'dq'' dq" dq'^dq'^ di^ dq'' Qg" dq'^dq'' 
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Define VF., = (g||j), 



where = q" — G" . Then we can write (5.12) as 



pi 



dL \ 92 $a 



dq°' I dq' 



Consider now the extended lagrangian L = L — AQ<i>" where Aq, — 



Pa- 



Then, the matrix (Wi 



^T§^j is equal, along Wi, to 



where W ah — 



It is easy see that the elements of the matrix (5.12) are given by 



■^ab = Wab -Wap^- W ^b^ + W 



dqb 



dq'' 



Qqa Qqb 



(5.16) 



(5.17) 



Now, using elemental linear algebra it is easy to show that matrix (5.17) is regular if and only 
if the matrix of elements (5.16) is regular. 



Example 5.3. The Cart-Pole System (see [7] and references therein). A Cart-Pole System 
consists of a cart and an inverted pendulum on it. The coordinate x denotes the position of 
the cart on the x-axis and 6 denotes the angle of the pendulum with the upright vertical. The 
configuration space is Q = M x 




First, we describe the Lagrangian function describing this system. The inertia matrix of the 
cart-pole system is given by 

mil = A/ + ni 
mi2{q2) = mi2{q2) ^ ml cos{d) 
TO22 = rnP 

where M is the mass of the cart and m, I are the mass, and length of the center of mass of 
pendulum, respectively. The potential energy of the cart-pole system is V{9) — mglcos{9). 
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The Lagrangian of the system (kinetic energy minus potential energy) is given by 



L{q, q) = L{x, 0, x, 9) = ^Mx^ + ^m{x^ + 2x16 cos + l^O"^) - mgl cos - mgh 



where h is the car height. 

The controller can apply a force F, the control input, parallel to the track remaining the joint 
angle 9 unactuated. Therefore, the equations of motion of the controlled system are 

[M + m)x — mW^ sin 9 + mW cos 9 = u 
X cos 9 + 19 — g sin 9 = 

Now we look for trajectories {x{t),9{t)),u{t)) on the state variables and the controls inputs 
with initial and final conditions, (a;(0), 6'(0), i(0), ^(0)), {x{T),9{T),x{T),9{T)) respectively, and 
minimizing the cost functional 

1 

A = - u^dt. 

Following our formahsm this optimal control problem is equivalent to the constrained second- 
order variational problem determined by 



f 

Jo 



L{x, 9, X, 9, X, 9) 



and the second-order constraint 

<^{x, 6, X, 0, X, 0) = X cos6 + 10 - g sin0 = , 

where 



(M + m)x — ml9^ sin 6 + ml0 cos 



We rewrite the second-order constraint as 

g sin — X cos ( 







Thus, the submanifold M of T(2)(M x S^) is given by 

M= ^{x,0,x,0,x, '0) I icos6'-h/6'-S'sin6' = o| . 

Let us consider the submanifold Wq = T*(r(M x §^)) X7'(rxsi) ^ with induced coordinates 
{x,0,x,0;pI,p%pI,pI,x). 

Now, we consider the restriction of L to M given by 



1 



X- , . />A9 , ^,osin0 — :rcos^, 
(M + m)x - mlsin99^ + ml cos 9{- ) 



(M + m)x — ml0^ sin + mg cos ^ sin ^ — mx cos^ 9 



For simplicity, denote by 



G 



gsin6 — X cos 
~ I ■ 
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Now, the presymplectic 2-forni ilw,,, the Hamiltonian ^nd the primary constraint ipl. are, 
respectively 



Hwn 



dx A +de h dpi + dx A dpi + dO A dpg , 



p'ix + p°g9 + pIx + pI 



g sm y — .T cos ( 
I 



1 

~2 
dH 



(M + m)x — mW^ sin 9 + mg cos 9 sin 9 — mx cos^ 



1 .dG'' OLm 
dx P^+P'^-^^""^ 



I.e., 



-Pe 



1 9G« (9L 



dx 



M 



dx 



This constraint determines the submanifold Wi. Applying Proposition |5. 1| we deduce that the 
2-form ilvFi, restriction of flwo to Wi, is symplectic since 



dx"^ 



dx^ 



[(Af + m) -TOCOS^ 9]^ ^0 . 



Therefore, the algorithm stabilizes at the first constraint submanifold Wi. Moreover, there 
exists a unique solution of the dynamics, the vector field X E X{Wi) which satisfies ix^Wi — 
dHwi ■ In consequence, we have a unique control input which extremizes (minimizes) the objective 
function A and then the force exerted to the car is the minimum possible. If we take the flow 



Ft : W\ Wi of the vector field X then we have that F^flwi 
function 



flwi ■ Obviously, the hamiltonian 



1 ^G" dL 



1 r 
2 



N 



dx dx 



{M + m)x — ml sin 99^ + mg cos 9 sin 9 — mx cos'^ 



x + Pe 



g sm ( 



is preserved by the solution of the optimal control problem, that is o Ft = H\-^ . Both 

properties, symplecticity and preservation of energy, are important geometric invariants. In 
next section, we will construct, using discrete variational calculus, numerical integrators which 
inherit some of the geometric properties of the optimal control problem (symplecticity, momentum 
preservation and, in consequence, a very good energy behavior). 



6. Geometric discretization of optimal control problems for underactuated 

mechanical systems 

6.1. Discrete vakonomic mechanics. In this section we discuss some ideas from discrete me- 
chanics for vakonomic systems (see fJ). The main idea is to use discrete variational calculus. 
In the case of vakonomic systems, the principle seeks to find a discrete curve which is a critical 
point of the discrete action sum subject to some constraint functions. The discrete curves are 
sequences of points that approximate curves on Q. 

The discretizing procedure of a given continuous vakonomic system, determined by a La- 
grangian function L : TQ R and a constraint submanifold M of TQ, starts first substituting 
the velocity phase space TQ by the cartesian product of two copies of Q, Q x Q. Secondly, 
we discretize the continuous lagrangian and the constraint submanifold to a discrete lagrangian 
function '■ Q x Q R and a constraint submanifold of Q x Q determined by the vanishing 
of m-independent constraints functions ■ Q x Q ^ M.. 
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Given and g^r in Qi for some (also fixed) integer N, we consider the space of sequences 
(go J Qii ■ ■ ■ T tQn) (the discrete paths joining qo to Qn)- We form the discrete action sum 



Ad{qo,qi, ■■■,qN) 



E 

fc=0 



Ld{qk,qk+i) 



over discrete paths satisfying the discrete constraints equations, that is '^'^{qk,qk+i) = 0, with 
fc = 0,...,A^— 1. We compute the critical point of this action sum subjected to the constraint 
equations; that is, 



with go and q^ fixed 
l<a<mand 0<fc<iV-l 



min Ad{qo,qi, ■.■,qN) 
suject to $2(9fe,gfc+i) = 0, 

Observe that system is subjected to Nm constraint functions. 

We define the augmented Lagrangian Ld'- Q y. Q y. M™ ^ M by 

Ld{x, y, A) = Ld{x, y) + K^di^, y). 

This Lagrangian Ld gives rise to the following unconstrained discrete variational problem 



(6.1) 



min Ad {qo,qi,- 

qk e Q, Afc e 



, qNi A", A\ . 
fc = 0, . 



, A^ ^) with go f^nd gjv fixed 
,iV-l, qN^Q, 



(6.2) 



where 



Ad (go,gi,..-,gjv,A°,A\...,A^-i) 



Af-l 

E 

k=0 
N-1 

E 

fe=0 



^d(gfc,gfc+i, a'') 



[Ldiqk,qk+i) + >^iK{qk,qk+i)] , 



where A*^ is a to- vector with components A^ with 1 < a < m. 

From the classical lagrangian multiplier theorem, we have that the regular extremals of Prob- 
lem (6.1) are the same than in Problem (6.2 1. Therefore, applying standard discrete variational 
calculus we deduce that the solutions of problem (6.1 1 verify the following set of difference equa- 
tions 

DiLdiqk, gfc+i) + D2Ld{qk-uqk) + A^Di$2(gfc, g^+i) + A^-iDa^S fe-i, ^fc) = , 1 < fc < TV - 1 
$^(gfc, gfc+i) = 0, l<a<TO, andO<fc<Ar-l 

where DiLd and D2Ld denote the derivatives of the discrete lagrangian Ld respect to the first 
and the second argument, respectively. 



For all function F e C°°(Q x Q) we denote by D12F the n x n-matrix 1 „ „ ■ 
derivatives with respect to the first and second variables). Then, if the matrix 

/ 



(partial 



Di2Ld + KDi2n 



dx 



dy 



Or. 



I in 



(n+m) X (n+m) 



is regular along x M™, then, by a direct application of the implicit function theorem, we 
deduce that there exists a unique map 



(a;,2/, A) 

such that for all solutions (go, gi, . . . , g^r, A*^, A^, 

Td(gfc-i,gfe,A^-i) 



{y,v,A) , 



, ^) of the equation (6.1 ) we have that 
{qk,qk+i,X'') ■ 

The application will be called the discrete flow of the vakonomic problem. 
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In [3], it is shown that the discrete flow preserves a symplectic form naturally defined 
on Md X and it is momentum preserving if the discrete Lagrangian Ld and the constraint 
submanifold are invariant under the action of a Lie group of symmetries. 

6.2. Discrete second-order vakonomic mechanics. In this subsection, we study discrete 
second-order mechanical systems with constraints (see [6J) bearing in mind the discretization of 
optimal control problems for underactuated mechanical systems. 

A natural discrete space substituting the second-order tangent bundle T^'^^Q is QxQxQ and 
therefore a discrete vakonomic system is determined by a discrete lagrangian : QxQxQ — > M 
and a constraint submanifold is locally determined by the vanishing of m-constraint functions 
: Q X Q X Q ^ M. 

Given a discrete lagrangian Ld Q x Q x Q — > M we define the discrete action Ad '■ Q^^^ 
R by 

N-2 

Ad{qo, ■■■,qN) = ^ Ld{qk,qk+i,qk+2)- 

k=0 

Adding the constraints, we have the following discrete constrained variational problem 
min Ad{qo,qi,--,qN), with go, 91 and qN^i,qN Axed 

subject to ^2ilk,qk+i,qk+2) = , with 1 < a < m, 0< k < N ~ 1. 



(6.3) 



As in the previous section we define the augmented lagrangian Ld Q x Q x Q x M™ M 
given by 

^d{qk,qk+i,qk+2,>^^) = Ld{qk,qk+i,qk+2) + Xa'^d{ik,qk+i,qk+2) 

with qk e Q; A'' = (Ai,...,A^J e M™, < fc < iV — 2. Hence, as in the previous section, 



Problem (6.3) is equivalent to the following (singular) unconstrained problem for La 



minyid (go,9i,---,9Af,A",A\...,A^ ^) with go, 91 and ^at^i, fixed , , . 

{qk,qk+i,qk+2)eQxQxQ, A^ G M™ , fc - 0, . . . , TV - 2, ^"'^ 



where 



N-2 

(go,9i,---,gAr,A",A\...,A^"^) = Ld{qk,qk+i,qk+2,X'') 



/c=0 
N-2 

E 

k=0 



^Ldiqk,qk+i,qk+2) + Xi<^di<lk,qk+i,qk+2) 



and A*^ is a m-vector with components Ajj , 1 < a < m. 
Hence, the extremality conditions are 

= D3Ld{qk-2,qk-i,qk) + D2Ld{qk-i,qk,qk+i) 

+DiLd{qk,qk+i,qk+2) + X^^^ D3<^>2{qk-2, qk-i, qk) 
+Xi-^D2<i>2{qk-i,qk:qk+i) + XiDi<i>2{qk,qk+i,qk+2), 

= *d(gfc-2,gfc-i,gfc) 

= ^diQk-i,qk,qk+i) 

= 'i'di<ik,qk+i,qk+2) ■ 

where 2 < k < N ~2. 
If the matrix 

det( Di3Ld{x,y,z) + XcDi3<i>2{x,y,z) D3<i>2{x,y, z) \ , ^ ,^ 
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is regular for all (a;, y, z) G = {{x, y, z) € QxQxQ \ <i>J^(a;, y, z) = 0} and Aq, S M, 1 < a < m. 
Assuming this regularity assumption and by a direct application of the implicit function theorem, 
we deduce that there exists a unique application 

Td : Md X — > MdX M^™ 

(90,91,92,93,^^, A;^) I — > (91,92,93,94, A^,A2) 

which univocally determines and X^, 1 < a < m from the initial conditions (go, 9i, 92, 93, -^a, '^a)- 
Here, denotes the submanifold oiQ'^ — QxQxQxQ 

= {(90,91,92,93) e I «'S(9o,9i,92) = 0,$;j(9i,g2,93) = 0, 1 < a < m} . 

The mapping will be called the discrete second-order vakonomic flow. 

Using similar techniques than in [U it is possible to show that, under the regularity as- 
sumptions, the discrete second-order vakonomic flow is symplectic and preserves momentum in 
the case when we have a Lie group action preserving the discrete lagrangian and the constraint 
submanifold M^. 

6.3. Application to optimal control of underactuated mechanical systems. In this sec- 
tion, we show that the discrete vakonomic approach of second order problem is an appropriate 
framework for discrete versions of optimal control problems of underactuated mechanical systems 
considered in Section [s] (see [28j for an alternative approach) . The main application will be the 
explicit construction of geometric numerical integrators for this type of systems. 

Let us take a discrete lagrangian : Q x Q M where Q — Qi x Q2 as in Section [U Then, 
an element {qQ,qf) € Q x Q admits a global decomposition of the form {qQ,qQ ,qi,qf). Thus, 
we can consider the following discrete underactuated mechanical system 

D^,L,{qti,qt) + DtUqt<lt+i) = < 

DqLd{qti,qt) + D^Uqt,qt+l) = 0, 

1 < A < n, 1 < a < m, m + 1 < a < n. Here DfLd and DfLd represent the partial derivatives 
with respect to coordinates a and a, respectively. 

The optimal control problem is determined prescribing the discrete cost functional 

Af-l 
k=l 

with initial and final conditions 90,91 and qN-i,qN, respectively. 

Since the control variables appear explicitly the previous discrete optimal control problem is 
equivalent to the second-order discrete vakonomic problem determined by 

Xd(9fe-i,9fc,9^+i) = C{qtqt+i,D^Ld{qti,qt) + DlLd{qtqk+i)) 
1>rf(9^l, 9^+1, 9^+2) = DqLd{qtl,qt)+D'lLd{qtqt+l) = ^- 
Applying the techniques developed in Subsection |6.2| and assuming the regularity condition 



(6.51 we obtain the discrete flow 

Td : Md X M^™ — > Md X M^™ 



Example 6.1. The Discrete Cart-Pole System: (See Example 5.3) 



Consider the following discrete Lagrangian Ld : Q x Q ^ K where Q = M x 
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where h is the car height. 

The discreted controlled Euler-Lagrange equations are 

{2Xk+l -Xk- Xk+l){ To — ) 



ml 



cos 



dk+l + Ok 



+ 



^2 {29k+l -Ok- ek+2) + ^2 



Img 



{0k+i - Ok) - cos 
Im 



— ^fe+l) 



Uk 



{Xk+\ - Xk) COS ^ ^fc+l^+^fc ^ _ - Xk+l) COS ^ 



9k+2 + Ok+1 



2 

Im 
2h? 



in I I + sm I 



sin 



V" 



V 



{xk+i - Xk){Ok+i - Ok) sin ^ ^fe+i^+^fc ^ + - Xk+i){0k+2 - Ok+i) sin ^ ^^+2 + ^k+i ^ 



For solving the associated discrete optimal control problem, we need to find sequences {{qk, Qk+i, Uk) }, 
minimizing the cost functional 

Af-l 



k=l 

We know that this problem is equivalent to solve the following variational problem with con- 
straints 

N-2 

minA^ = ^ L{xk,0k,Xk+i,0k+i,Xk+2,0k+2) 
subject to the constraints 



^d{qk,qk+i,qk+2) = 



fc+i 



9fc - dk+2) + Im 



+ 



Imgh 



2 r 



sm 



^fc+l + k 



{xk+1 - Xk)cos 

Ok+2 + Qk+l 



Ok+1 + Qk 



- {Xk+2 - Xk+l) COS 



sm 



Im 



{xk+i - Xk){Ok+i - Ok) sin 



/ 9k+i + Ok 
V 2 



with k = Q,...,N -2 where : Q x Q x Q 



+ ixk+2 - Xk+i){0k+2 - Ok+i) sin 
is given by 



0k-r2 + ^A: + l 



Ok+2 + Ok+1 



Ld{qk,qk+i,qk+2) 



2% 

(M + m)2 



2/i4 

(mrf 



{2xk+i - Xk - Xk+2)'^ 

(Ok+i -Ok)- cos 



cos 



'k+2 + Ok+i 

2 



{Ok+2 — Ok+i] 



4+1 + Ok 

2^ 

To test our numerical algorithm we have programmed it in Matlab. The program admits as input 

data {qo,qi,q2,q3,\Q,\i) and the number of steps N; then it computes q4 and A2 and replace 
the initial data with {qi, q2, qa, qi, ^1, ^2) to calculate q^ and A3, etc. 
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